﻿using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Threading;
using SWS;

public enum ParentNodeDir
{
    Straight = 0,
    Right,
    Return,
    Left
}

public class NavigationManager : MonoBehaviour
{
    #region 测试

    public RoadNode from_1;
    public RoadNode to_1;

    public RoadNode from_2;
    public RoadNode to_2;

    public AI_CarController car1;
    public int car1Index;

    public AI_CarController car2;
    public int car2Index;
    #endregion
    
    public Dictionary<int,AI_CarController> SplineMoveDic = new Dictionary<int, AI_CarController>();

    private void Start()
    {
        //测试
        SplineMoveDic.Add(car1Index, car1);
        SplineMoveDic.Add(car2Index, car2);
    }

    private void Update()
    {
        if (Input.GetKeyDown(KeyCode.M))
        {
            BeginCaculateThread(car1Index, from_1, to_1);
            BeginCaculateThread(car2Index,from_2,to_2);
        }
    }
    /// <summary>
    /// 获取路径列表的主函数
    /// </summary>
    /// <param name="startPathNode">起始路径节点</param>
    /// <param name="endPathNode">结束路径节点</param>
    /// <returns></returns>
    public void BeginCaculateThread(int carIndex, RoadNode startPathNode, RoadNode endPathNode)
    {
        StartCoroutine(Caculate(carIndex, startPathNode,endPathNode));
    }

    /// <summary>
    /// 开启线程计算路径,计算完成则开始行驶
    /// </summary>
    /// <param name="carIndex">车辆下标</param>
    /// <param name="startPathNode">起始路径节点</param>
    /// <param name="endPathNode">结束路径节点</param>
    /// <returns></returns>
    IEnumerator Caculate(int carIndex, RoadNode startPathNode, RoadNode endPathNode)
    {
        List<RoadNode> openList = new List<RoadNode>();
        List<RoadNode> closeList = new List<RoadNode>();
        //讲计算数据装入当前路径中
        PutInfoInNode(null, 0, carIndex, endPathNode, startPathNode);
        //添加第一个路径节点
        openList.Add(startPathNode);
        List<PathManager> pathlist = SearchPathCircleFunction(carIndex, openList, closeList, endPathNode);
        yield return pathlist;
        print("找到路径,数量为:"+ pathlist.Count);
        SplineMoveDic[carIndex].SetCarMoveStep(pathlist);
    }

    /// <summary>
    /// 循环搜索路径函数
    /// </summary>
    /// <param name="carIndex">车辆下标</param>
    /// <param name="openList">开放列表</param>
    /// <param name="closeList">关闭列表</param>
    /// <param name="endPathNode">结束路径节点</param>
    /// <returns>结算结果</returns>
    public List<PathManager> SearchPathCircleFunction(int carIndex, List<RoadNode> openList,List<RoadNode> closeList,RoadNode endPathNode)
    {
        RoadNode node = null;
        while (openList.Count != 0)
        {
            //提取出开放列表中距离最小的一个路径节点
            node = openList.OrderBy(_node => _node.CostInfoDic[carIndex].costLength).First();
            if (node == endPathNode)
            {
                //找到最佳方法
                //print("找到最佳路径,结束搜寻");
                return GetNodeToList(node, carIndex);
            }
            //从开放列表中移除并添加到关闭列表
            openList.Remove(node);
            closeList.Add(node);
            //遍历提取出来的路径节点的下级节点
            for (int i = 0; i < node.NextNodes.Length; i++)
            {
                //该节点不能为空，不能已经在开放列表和关闭列表中，否则是已经计算过的路径，无需再计算
                if (node.NextNodes[i] != null && !openList.Contains(node.NextNodes[i]) && !closeList.Contains(node.NextNodes[i]))
                {
                    PutInfoInNode(node, i, carIndex, endPathNode);
                    openList.Add(node.NextNodes[i]);
                    //print("添加新路径到开放列表");
                }
            }
        }
        if (node!= endPathNode)
        {
            Debug.LogError("无法找到最佳路径");
        }
        //print("遍历完毕，返回最佳路径");
        return GetNodeToList(node,carIndex);
    }

    /// <summary>
    /// 将数据存入目标路径，并计算cost
    /// </summary>
    /// <param name="parentNode">父路径节点</param>
    /// <param name="childRoadNodeIndex">子路径下标</param>
    /// <param name="carIndex">车辆id</param>
    /// <param name="endPathNode">结束路径节点</param>
    public void PutInfoInNode(RoadNode parentNode,int childRoadNodeIndex, int carIndex,RoadNode endPathNode,RoadNode DefaultNode = null)
    {
        if (parentNode == null&& DefaultNode!=null)
        {
            //print("设置起始路径信息");
            DefaultNode.CalculateLength(carIndex, SplineMoveDic[carIndex].transform.position, endPathNode.SelfPath.waypoints[endPathNode.SelfPath.waypoints.Length - 1].position,null,null);

            return;
        }
        //print("设置下级路径信息");
        parentNode.NextNodes[childRoadNodeIndex].CalculateLength(carIndex, SplineMoveDic[carIndex].transform.position, endPathNode.SelfPath.waypoints[endPathNode.SelfPath.waypoints.Length - 1].position, parentNode, parentNode.NextNodeCurve[childRoadNodeIndex]);
    }

    /// <summary>
    /// 根据最后一个路径节点和车辆id,向上查找路口弯道，父路径
    /// </summary>
    /// <param name="lastNode">路径节点</param>
    /// <param name="carIndex">车辆ID</param>
    /// <returns>返回逆向路径列表</returns>
    public List<PathManager> GetNodeToList(RoadNode lastNode,int carIndex)
    {
        List<PathManager> list = new List<PathManager>();
        list.Add(lastNode.SelfPath);
        RoadNode parentNode = null;
        PathManager parentPathCurve = null;
        do
        {
            parentNode = lastNode.CostInfoDic[carIndex].parentNode;
            if(parentNode == null) break;
            parentPathCurve = lastNode.CostInfoDic[carIndex].parentPathCurve;
            list.Add(parentPathCurve);
            list.Add(parentNode.SelfPath);
            lastNode = parentNode;
        } while (true);
        return list;
    }
}
